#ifndef __QUATERNION_H__
#define __QUATERNION_H__

#include "common/vector3.h"

struct Quaternion {
    explicit inline Quaternion() {}
    inline Quaternion(const float _w, const Vector3& v) {
        hwVector = (HwVector) {v.x, v.y, v.z, _w};
    }
    inline Quaternion(const float _w, const float _x, const float _y, const float _z) {
        hwVector = (HwVector) {_x, _y, _z, _w};
    }
    inline Quaternion(const Quaternion& q) {
        hwVector = q.hwVector;
    }

    //assignment operators
    inline void operator=(const Quaternion& q){
        hwVector = q.hwVector;
    }

    inline Vector3 toVector() const {
        Vector3 result;
        result.hwVector = hwVector;
        return result;
    }

    union {
        struct {
            float x;
            float y;
            float z;
            float w;
        };
        float array[4];
        HwVector hwVector;
    };
};

inline Quaternion  operator*(const Quaternion& q1, const Quaternion&  q2) {
    Quaternion result;
    // TODO: use vector math if needed
    result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
    result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z;
    result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x;
    result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
    return result;
}

inline Quaternion conjugate(const Quaternion& q) {
    Quaternion result;
    // TODO: use vector math if needed
    result.x = -q.x;
    result.y = -q.y;
    result.z = -q.z;
    result.w =  q.w;
    return result;
}

inline Vector3 twist(const Quaternion& q, const Vector3& v) {
    Quaternion p(0, v);
    // FIXME: in original source it was like this, q'*p*q; should it be q*p*q'? or (q*p*q')/(||q||^2)?
    Quaternion result = conjugate(q) * p * q;
    return result.toVector();
}

#endif // __QUATERNION_H__
